#include <iostream> // 输入输出流库
#include <thread> // 线程库
#include <pcl/io/pcd_io.h> // 点云PCD文件读写头文件
#include <pcl/point_types.h> // 点类型头文件
#include <pcl/registration/ndt.h> // NDT算法头文件
#include <pcl/filters/approximate_voxel_grid.h> // 体素格滤波器头文件
#include <pcl/visualization/pcl_visualizer.h> // 点云可视化头文件

using namespace std::chrono_literals; // 使用chrono命名空间中的字面量

int main(int argc, char **argv) // 主函数
{
    // 创建目标点云对象的智能指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // 读取目标点云文件
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/room_scan1.pcd", *target_cloud) == -1)
    {
        PCL_ERROR("Could not find pcd \n"); // 如果文件不存在，输出错误信息
        return (-1); // 返回错误码
    }
    std::cout << "load " << target_cloud->size() << " data points from target_cloud" << std::endl; // 输出目标点云的点数

    // 创建源点云对象的智能指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // 读取源点云文件
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/room_scan2.pcd", *source_cloud) == -1)
    {
        PCL_ERROR("Could not find pcd \n"); // 如果文件不存在，输出错误信息
        return (-1); // 返回错误码
    }
    std::cout << "load " << source_cloud->size() << " data points from source_cloud" << std::endl; // 输出源点云的点数

    // 创建滤波后的点云对象的智能指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // 创建体素格滤波器对象
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    // 设置滤波器的体素大小
    approximate_voxel_filter.setLeafSize(0.3, 0.3, 0.3);
    // 设置输入点云并进行滤波
    approximate_voxel_filter.setInputCloud(source_cloud);
    approximate_voxel_filter.filter(*filter_cloud);

    std::cout << "Filter cloud contain " << filter_cloud->size() << "data points from source_cloud" << std::endl; // 输出滤波后的点云点数

    // 设置初始旋转和平移变换
    Eigen::AngleAxisf init_rotation(0.69, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translasition(1.0, 0, 0);
    Eigen::Matrix4f init_guss = (init_translasition * init_rotation).matrix();

    // 创建输出点云对象的智能指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // =======================  PCL实现 ndt   =======================
    // 创建NDT对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    // 设置NDT算法参数
    ndt.setTransformationEpsilon(0.01);
    ndt.setStepSize(0.1);
    ndt.setResolution(1.0);
    ndt.setMaximumIterations(35);
    // 设置输入源点云和目标点云
    ndt.setInputSource(filter_cloud);
    ndt.setInputTarget(target_cloud);
    // 执行NDT算法
    ndt.align(*output_cloud, init_guss);
    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged() // 输出NDT算法是否收敛
              << " score: " << ndt.getFitnessScore() << std::endl; // 输出拟合分数
    std::cout << "ndt transformation: \n"
              << ndt.getFinalTransformation() << std::endl; // 输出最终变换矩阵

    // =======================   ndt   =======================

    // 创建可视化对象
    pcl::visualization::PCLVisualizer::Ptr viewer_final(new pcl::visualization::PCLVisualizer("3D viewer"));
    // 设置背景颜色为黑色
    viewer_final->setBackgroundColor(0, 0, 0);
    // 设置目标点云颜色为红色并添加到可视化对象中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target_cloud");
    // 设置目标点云的点大小
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target_cloud");

    // 设置输出点云颜色为绿色并添加到可视化对象中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output_cloud");
    // 设置输出点云的点大小
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output_cloud");
    // 添加全局坐标系
    viewer_final->addCoordinateSystem(1.0, "global");
    // 初始化相机参数
    viewer_final->initCameraParameters();

    // 可视化循环
    while (!viewer_final->wasStopped())
    {
        viewer_final->spinOnce(100); // 可视化对象进行一次渲染
        std::this_thread::sleep_for(100ms); // 休眠100毫秒
    }
    return 0; // 返回0，程序结束
}